/* 
 * File:   Contact.cpp
 * Author: RedEyedKiller
 * 
 * Created on 6 Ιούλιος 2011, 5:28 πμ
 */

#include "Contact.h"
#include "EntityContactInfo.h"
#include "../Logger.h"
#include "TileContactInfo.h"

namespace physicsSystem
{

namespace PhysicsInternals
{

Contact::Contact(EntitySystem::PhysicsLogic* first, EntitySystem::PhysicsLogic* second, const Math::Vector2F& info, int groupIndex)
{
    group = groupIndex;
    this->first = first;
    this->second = second;

    if (!first->IsAwake())
    {
        first->Awake();
    }

    if (!second->IsAwake())
    {
        second->Awake();
    }

    //calculate contact normal
    Math::Rect pollingRect1(*(first->GetRect()));
    pollingRect1.SetOrigin(first->GetPosition().ConvertTo<int>());

    Math::Rect pollingRect2(*(second->GetRect()));
    pollingRect2.SetOrigin(second->GetPosition().ConvertTo<int>());

    Math::Vector2I commonPoints[2];

    if (!Math::CommonPoints(pollingRect1, pollingRect2, commonPoints))
    {
        Math::CommonPoints(pollingRect2, pollingRect1, commonPoints);
    }

    contactNormal = (commonPoints[0] - commonPoints[1]).ConvertTo<float>();

    //flip members and get their absolute values
    contactNormal.Absolute();
    //and flip the members
    contactNormal.Flip();

    //nulify the lesser one
    if (contactNormal.GetX() > contactNormal.GetY())
    {
        contactNormal.Set(1.0f, 0.0f);
    }
    else if (contactNormal.GetX() < contactNormal.GetY())
    {
        contactNormal.Set(0.0f, 1.0f);
    }
    else
    {
        contactNormal.Set(0.5f, 0.5f);
    }

    Math::Vector2F pen = (info * contactNormal);
    penetration = pen.Length();

    Math::Vector2F dC = pollingRect1.GetCenter() - pollingRect2.GetCenter();

    dC *= contactNormal;

    dC.Normalize();

    contactNormal *= dC;

    restitution = first->GetMaterial().GetBounceModifier() * second->GetMaterial().GetBounceModifier();

#ifdef DEBUG_DRAW
    RenderingManager::GetInstance()->RenderLine(pollingRect1.GetCenter(), pollingRect1.GetCenter() + contactNormal * 50, 0, 255, 0);
    RenderingManager::GetInstance()->RenderLine(pollingRect1.GetCenter(), pollingRect1.GetCenter() + pen * 50, 255, 0, 0);
#endif

    //contact information into contact list
    first->AddEntityContact(new EntityContactInfo(second->GetParent(), contactNormal, first->GetVelocity() - second->GetVelocity()));
    second->AddEntityContact(new EntityContactInfo(first->GetParent(), contactNormal * -1, second->GetVelocity() - first->GetVelocity()));

}

Contact::Contact(EntitySystem::PhysicsLogic* first, const Math::Vector2F& info, const Math::Vector2F& contactNormal_) : contactNormal(contactNormal_)
{
    group = first->GetGroupIndex();
    this->first = first;
    this->second = NULL;

    if (!first->IsAwake())
    {
        first->Awake();
    }

    Math::Vector2F pen = (info * contactNormal.GetAbsolute());
    penetration = pen.Length();

    restitution = first->GetMaterial().GetBounceModifier();

    first->AddTileContact(new TileContactInfo(contactNormal, 1));
}

void Contact::Resolve(float duration)
{
    ResolveInterpenetration(duration);
    ResolveVelocity(duration);
}

float Contact::GetSeparatingVelocity()
{
    if (second && (first->IsGhost() || second->IsGhost()))
        return 0;

    Math::Vector2F relVel = first->GetVelocity();
    if (second)
        relVel -= second->GetVelocity();

    return relVel.DotProduct(contactNormal);
}

void Contact::ReportScripts()
{
    if (!second)return;
    first->OnEntityCollision(second->GetParent(), contactNormal);
    second->OnEntityCollision(first->GetParent(), contactNormal * -1);
}

void Contact::ResolveInterpenetration(float duration)
{
    if (penetration <= 0)
    {
        //nullify movement
        movement[0] = Math::Vector2F::Zero;
        movement[1] = Math::Vector2F::Zero;
        return;
    }

    float totalImass = first->GetInvertedMass();
    if (second)
        totalImass += second->GetInvertedMass();

    if (totalImass <= 0)
    {
        //nullify movement
        movement[0] = Math::Vector2F::Zero;
        movement[1] = Math::Vector2F::Zero;
        return;
    }

    //calculate the percentage of movement in contact direction.
    Math::Vector2F movePerIMass = contactNormal * (penetration / totalImass);

    first->AdjustPositionPerImass(movePerIMass);
    movement[0] = movePerIMass * first->GetInvertedMass();

    if (second)
    {
        movement[1] = movePerIMass * - second->GetInvertedMass();
        second->AdjustPositionPerImass(movePerIMass * -1);
    }
    else
    {
        movement[1] = Math::Vector2F::Zero;
    }
}

void Contact::ResolveVelocity(float duration)
{
    float sepVelocity = GetSeparatingVelocity();
    if (sepVelocity >= 0)
    {
        return;
    }

    float newSepVel = -sepVelocity * restitution;

    Math::Vector2F accCausedVelocity = first->GetGeneralAcceleration();

    if (second)
        accCausedVelocity -= second->GetGeneralAcceleration();

    float accCausedSepVelocity = accCausedVelocity.DotProduct(contactNormal) * duration;

    // If we've got a closing velocity due to acelleration build-up,
    // remove it from the new separating velocity
    if (accCausedSepVelocity < 0)
    {
        newSepVel += restitution * accCausedSepVelocity;

        // Make sure we haven't removed more than was 
        // there to remove.
        if (newSepVel < 0) newSepVel = 0;
    }

    float deltaVelocity;
    if (sepVelocity > -0.1)
        deltaVelocity = -sepVelocity;
    else
        deltaVelocity = newSepVel - sepVelocity;

    float totalImass = first->GetInvertedMass();
    if (second)
        totalImass += second->GetInvertedMass();

    if (totalImass <= 0)
        return;

    Math::Vector2F impulse = contactNormal * (deltaVelocity / totalImass);

    first->AdjustVelocityPerImass(impulse);

    if (second)
        second->AdjustVelocityPerImass(impulse * -1);

    //this is usefull for reducing the jiger effect
    restitution *= 0.5;
}

}

}